
import time
import sys
import pickle
from threading import Thread
from traceback import print_exc

from Brain.vector import Vector
from Brain.plan import Plan, MovementStep, ChasepakStep, BuildStep, PlacelintelStep
from Brain.world import WORLD
from Brain.movement import MotorCortex
from Brain.arm.robot_arm import ArmControl
from Brain.sensors.encoder import Encoder
from Brain.sensors.ultrasound import UltrasoundModule
from Brain.operations.registration import OPERATIONS
from Brain.knowledge_database.match import Database
from Brain.vision import VisualCortex
from Brain.recording import History, BrainRecord

VERSION = '1.4.1'
TEST_SINGLE_PLAN = False

class PositionSynchronization(Thread):
	""" read encoder, ultrasound, ir devices and 
		calculate robot position
	"""
	ULTRASOUND_ERROR = 100
	
	def __init__(self, world, encoder, ultrasound, ir=None):
		Thread.__init__(self)
		self.encoder = encoder 
		self.ultrasound = ultrasound
		self.ir = ir 
		self._world = world
	
	def run(self):
		while 1:
			self.encoder.new_data_event.wait(1.2)
			self.encoder.new_data_event.clear()
			newdata = self.encoder.read()
			
			if self.ultrasound and self.ultrasound.new_data_event.isSet():
				self.ultrasound.new_data_event.clear()
				correction = self.ultrasound.read()
				diff = correction - newdata.pos 
				length = diff.length()
				if length > self.ULTRASOUND_ERROR:
					## snap to absolute sensor readings
					diff *= (length - self.ULTRASOUND_ERROR) / length
					newdata.pos += diff 
			
			self._world.lock.acquire()
			self._world.pos_x = newdata.pos.x
			self._world.pos_y = newdata.pos.y
			self._world.velocity_x = newdata.velocity.x
			self._world.velocity_y = newdata.velocity.y
			self._world.angle = newdata.angle
			self._world.lock.release()

class ChangePlanException(Exception):
	""" we need to change plan, contains details text """

class MatchEndException(Exception):
	""" Time has run out. """

class TimeIntervals:
	def __init__(self, **kw):
		self._min_execution = 0.0
		self._max_execution = 0.0
		self._begin = time.time()
		self.__dict__.update(kw)
	
	def change_intervals(self, min, max):
		self._min_execution, self._max_execution, = min, max,
	
	def timed_out(self):
		return (self._begin + self._max_execution) < time.time()

class ExecutingPlan(TimeIntervals):
	def __init__(self, plan):
		min, max = self.DEFAULT_BOTTOM, self.DEFAULT_TOP
		try:	max = getattr(step, 'timelimit')
		except: pass
		try:	min = getattr(step, 'timelimit_bottom')
		except: pass
		TimeIntervals.__init__(self, _min_execution=min, _max_execution=max)
		
		self._plan = plan
		self._stepindex = 0 
	
	def next_step(self):
		step = self._plan.get_step(self._stepindex)
		self._stepindex += 1
		if step == None:
			raise ChangePlanException('Plan %s Completed' % (str(self._plan), ))
		return step

class ExecutingStep(TimeIntervals):
	DEFAULT_BOTTOM = 0.0
	DEFAULT_TOP = 10.0
	def __init__(self, step):
		min, max = self.DEFAULT_BOTTOM, self.DEFAULT_TOP
		try:
			max = getattr(step, 'timelimit')
		except: 
			pass
		TimeIntervals.__init__(self, _min_execution=min, _max_execution=max)
		self.step = step
	def __str__(self):
		return 'plan: %s limits: %s - %s' % (self.step, self._min_execution, self._max_execution, )

class CentralCortex:
	""" central cortex
	"""
	SLEEP_INTERVAL = 0.05
	STRATEGY_PRESCALER = 5
	MATCH_DURATION = 90.0
	
	def __init__(self, match_filename):
		self._begin = 0.0
		self._executing_step = None
		self._executing_plan = None		#ExecutingPlan( pickle.load( open(plan_filename, 'rb') ) )
		self._history = History('brain')

		self._world = WORLD()
		self._database = Database(match_filename, self._world)	# load plans, setup start parameters
		
		self._worker = None			# executing operation worker
		
		self._encoder = Encoder()
		self._ultrasound = UltrasoundModule(self._world)
		self._movement = MotorCortex(self._encoder)
		self._vision = VisualCortex(self._world, self._movement)
		self._arm = ArmControl(self._world, self._vision)
		
		self._encoder.setDaemon(True)
		self._movement.setDaemon(True)
		self._ultrasound.setDaemon(True)
		self._arm.setDaemon(True)
		
		self._position_handler = PositionSynchronization(self._encoder, self._ultrasound)
		self._position_handler.setDaemon(True)

		self._peripherals = {
			'camera' : self._vision,
			'wheels' : self._movement,
			'arm' : self._arm,
			'encoder' : self._encoder,
			'ultrasound' : self._ultrasound,
			}
		
		self._operations = []		# operations currently active
		## handler is created for that specific task
		self._dynamic_operations = OPERATIONS
		## operations that do not create handler for the task ..
		self._static_operations = {
			MovementStep : self._movement,
			ChasepakStep : self._vision,
			BuildStep : self._arm,
			PlacelintelStep : self._arm,
			}
	
	def _reset(self):
		self._history.write( 'reset sequence ..' )
		
		self._begin = time.time()
		self._encoder.start()
		self._encoder.reset( Vector(self._world['start x'], self._world['start y']), self._world['angle'] )
		self._movement.start()
		self._ultrasound.start()
		self._arm.start()
		self._position_handler.start()
	
	def _shutdown(self):
		""" global restart """
		self._history.write( 'shutdown sequence ..' )
		if self._worker: self._worker.kill_task()
		for operation in self.operations:
			operation.kill_task()
		
		for name, module, in self._peripherals:
			module.dump_logs()
			module.cleanup()
		self._history.dump()
	
	def run(self):
		""" Updates world object from sensors
			handles active operations
			executes planed steps
		"""
		while 1:
			if self._check_start_signal():
				print 'go go go ...'
				break
			time.sleep(0.01)
		
		self._reset()
		
		strategy_iterations = range(7)
		preskaler_iterations = range(self.STRATEGY_PRESCALER)
		
		try:
			while 1:
				for i in preskaler_iterations:
					#self._apply_operations()
					self._handle_sensor_data()
					time.sleep(SLEEP_INTERVAL)
				
				for i in strategy_iterations:
					if self._process_plan(): 
						break
		except:
			print_exc()
			print '\n shuting down ..\n'
			self._shutdown()
			raise 
	
	def _process_plan(self):
		""" process main course of action ,
			handle time constraints 
		"""
		if self._world.time_passed > self.MATCH_DURATION:
			self._history.write( 'match end ..' )
			raise MatchEndException()
		
		try:
			if self._executing_plan == None:
				self._history.write( 'current plan is empty' )
				raise ChangePlanException(' - current plan is empty - ')
			
			if self._worker == None or self._worker.is_completed():
				## process next step
				next_step = self._executing_plan.next_step()
				if self._worker: self._worker.kill_task()
				
				if next_step.__class__ in self._static_operations:
					self._worker = self._static_operations[next_step.__class__]
				elif next_step.__class__ in self._dynamic_operations:
					worker_class = self._dynamic_operations[next_step.__class__]
					self._worker = worker_class(self._world, self._peripherals)
				else:
					raise ChangePlanException('- unknown step ! -')
				
				self._executing_step = ExecutingStep( next_step, 0, max )
				self._history.write( str(self._executing_step) )
				## start worker ..
				self._worker.start_task( next_step )
			else:
				## apply time constraints :
				if self._executing_step:
					if self._executing_step.timed_out():
						print ' + step timmed out ! '
						self._history.write( 'timed out step: %s' % (self._executing_step, ) )
						
						if self._worker: 
							self._worker.kill_task()
						
						self._force_next_step()
						return False
				else:
					raise ChangePlanException(' - EXECUTING_STEP MISSING ! - ')
		
		except ChangePlanException, ex:
			print ' + plan change: %s' % (str(ex), )
			self._evaluate_plans()
			return True
		except:
			print_exc()
			self._evaluate_plans()
			return True
		
		return True	# done
	
	def _force_next_step(self):
		self._worker = None

	def _check_start_signal(self):
		""" Should return true if starting cord is pulled
			ZBUDZ: always returns True, until we set up starting cord
		"""
		return True
	
	def _evaluate_plans(self):
		""" current plan has finished, choose another
		"""
		
		solutions = self._database.pick_plan( self._world ) 
		if len(solutions) > 0:
			new_plan = solutions[0][1]
			self._history.write( 'evaluate :' )
			
			for solution in solutions:
				self._history.write( 'rank: %s  plan: %s' % (solution[0], solution[1],) )
			
			self._history.write( 'plan changed to %s with priority: %s' % (new_plan, solutions[0]) )
			self._executing_plan = ExecutingPlan( new_plan )
			self._executing_step = None
			if self._worker: 
				self._worker.kill_task()
			self._worker = None
		else:
			raise Exception(' - NO PLANS LEFT !!!! - ')
	
	def _apply_operations(self):
		""" not used, operations are asynchrous
		"""
	
	def _handle_sensor_data(self):
		""" Update self.world parameters.
			
			- only encoder data is processed !
		"""
		self._world.time_passed = time.time() - self._begin 
		self._world.time_left = self.MATCH_DURATION - self._world.time_passed 

usage = """
	usage:
		brain.py "path to game file"
"""

if __name__ == '__main__':
	print 'awake ..'
	print 'Barabe central cortex ver %s' % (VERSION, )
	
	
	if len(sys.argv) < 2:
		#print 'no starting set specified.'
		print usage
		sys.exit()

	filename = sys.argv[1]
	
	print 'execute', filename 
	
	logic = CentralCortex(filename)
	logic.run()
	print 'done.'






